package com.example.agsensor;

import androidx.appcompat.app.AppCompatActivity;

import android.content.Context;
import android.os.Bundle;
import android.os.Handler;
import android.os.Message;
import android.util.Log;
import android.widget.TextView;
import com.example.agsensor.databinding.ActivityMainBinding;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

public class MainActivity extends AppCompatActivity implements SensorEventListener{
    private static final String TAG = MainActivity.class.getSimpleName();

    // Used to load the 'agsensor' library on application startup.
    static {
        System.loadLibrary("agsensor");
    }

    private ActivityMainBinding binding;

    private TextView textView;
    private TextView textView2;

    private SensorManager sensorManager;
    private Sensor accelerometerSensor;
    private Sensor gyroscopeSensor;
    // 将纳秒转化为秒
    private static final float NS2S = 1.0f / 1000000000.0f;

    private float timestamp;
    private float angle[] =new float[3];
    float accel_x = 0;
    float accel_y = 0;
    float accel_z = 0;
    float gyro_x = 0;
    float gyro_y = 0;
    float gyro_z = 0;
    float[] accelArray = new float[3];
    float[] gyroArray = new float[3];

    float[] accelArray2 = new float[3];
    float[] gyroArray2 = new float[3];

    float[] gravity = new float[3];

    private static final int MSG_ID_DO_UPDATE = 1;
    private static final int MSG_ID_UPDATE_accel = 2;
    private static final int MSG_ID_UPDATE_gyro = 3;
    private final Handler handler = new Handler() {
        @Override
        public void handleMessage(Message msg) {
            switch(msg.what) {
                case MSG_ID_DO_UPDATE:
                    handler.removeMessages(MSG_ID_DO_UPDATE);

                    final float[] angleArray = doMahonyAHRSupdateIMU(accelArray2,gyroArray2,0/*0:SixAxisImuPose or 1:MahonyAHRS or 2: 一阶互补滤波 3: 卡尔曼滤波*/);

                    String text = String.format("IMU data: \ntime:%d,\n a_x: %5.5f \na_y: %5.5f, \na_z: %5.5f, \ng_x: %5.5f, \ng_y: %5.5f, \ng_z: %5.5f\r\n", System.currentTimeMillis(), accelArray2[0], accelArray2[1], accelArray2[2], gyroArray2[0], gyroArray2[1], gyroArray2[2]);
                    text += "\n";
                    //text += "Angle：\npitch=" + Math.toDegrees(angleArray[0]) + ", \nyaw="+Math.toDegrees(angleArray[1])+", \nroll="+Math.toDegrees(angleArray[2]);
                    text += "Angle：\nroll(y)="+angleArray[0] + ", \npitch(x)=" + angleArray[1] + ", \nyaw(z)="+angleArray[2];
                    textView.setText(text);

                    handler.sendEmptyMessageDelayed(MSG_ID_DO_UPDATE,10);
                    break;
                case MSG_ID_UPDATE_accel:
                    accelArray2 = (float[])msg.obj;
                    break;
                case MSG_ID_UPDATE_gyro:
                    gyroArray2 = (float[])msg.obj;
                    break;
            }
        }
    };

    /*
    陀螺仪: gyroscope   /ˈdʒaɪrəskoʊp/
    重力:  gravity  /ˈɡrævəti/
    加速度计: accelerometer:  /əkˌseləˈrɑːmɪtər/
    磁力计: magnetometer /ˌmæɡnəˈtɑːmɪtər; ˌmæɡnəˈtɑːmətər /
     */
    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);

        binding = ActivityMainBinding.inflate(getLayoutInflater());
        setContentView(binding.getRoot());

        gravity[0]=0;gravity[1]=0;gravity[2]=0;

        // Example of a call to a native method
        textView = binding.sampleText;
        textView.setText(stringFromJNI());

        textView2 = binding.textView;

        sensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);

        accelerometerSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        gyroscopeSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);

        sensorManager.registerListener(this,gyroscopeSensor, SensorManager.SENSOR_DELAY_NORMAL);
        sensorManager.registerListener(this,accelerometerSensor, SensorManager.SENSOR_DELAY_NORMAL);

        handler.sendEmptyMessageDelayed(MSG_ID_DO_UPDATE,50);
    }

    @Override
    public void onDestroy() {
        super.onDestroy();

        sensorManager.unregisterListener(this);
    }

    //坐标轴都是手机从左侧到右侧的水平方向为x轴正向，从手机下部到上部为y轴正向，垂直于手机屏幕向上为z轴正向

    @Override
    public void onSensorChanged(SensorEvent event) {
        if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            /*
            All values are in SI units (m/s^2)
            values[0]: Acceleration minus Gx on the x-axis
            values[1]: Acceleration minus Gy on the y-axis
            values[2]: Acceleration minus Gz on the z-axis
            */
            //为了测量仪器的实际加速度，必须消除重力的作用。这可以通过应用高通滤波器来实现。相反，低通滤波器可以用来隔离重力。
            // alpha is calculated as t / (t + dT)
            // with t, the low-pass filter's time-constant
            // and dT, the event delivery rate
            final float alpha = 0.8f;
            float[] linear_acceleration = new float[3];
            gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
            gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
            gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];
            linear_acceleration[0] = event.values[0] - gravity[0];
            linear_acceleration[1] = event.values[1] - gravity[1];
            linear_acceleration[2] = event.values[2] - gravity[2];


            // x,y,z分别存储坐标轴x,y,z上的加速度
            accelArray[0] = event.values[0];//m/s^2
            accelArray[1] = event.values[1];
            accelArray[2] = event.values[2];
            handler.sendMessage(handler.obtainMessage(MSG_ID_UPDATE_accel,accelArray));
            // 根据三个方向上的加速度值得到总的加速度值a
            //float a = (float) Math.sqrt(accel_x * accel_x + accel_y * accel_y + accel_z * accel_z);

            textView2.post(new Runnable() {
                @Override
                public void run() {
                    String text = String.format("linear_acceleration：\nAccel_x: %.5f \nAccel_y: %.5f \nAccel_z: %.5f",linear_acceleration[0],linear_acceleration[1], linear_acceleration[2]);
                    textView2.setText(text);
                }
            });
        }else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {

            //从 x、y、z 轴的正向位置观看处于原始方位的设备，如果设备逆时针旋转，将会收到正值；否则，为负值
            if(true /*timestamp != 0*/){
                // 得到两次检测到手机旋转的时间差（纳秒），并将其转化为秒
                //final float dT = (event.timestamp -timestamp) * NS2S;
                // 将手机在各个轴上的旋转角度相加，即可得到当前位置相对于初始位置的旋转弧度
                /*angle[0] += event.values[0] * dT;
                angle[1] += event.values[1] * dT;
                angle[2] += event.values[2] * dT;
                // 将弧度转化为角度
                gyro_x = (float) Math.toDegrees(angle[0]);
                gyro_y = (float) Math.toDegrees(angle[1]);
                gyro_z = (float) Math.toDegrees(angle[2]);*/

                gyroArray[0] = event.values[0];//rad/s
                gyroArray[1] = event.values[1];
                gyroArray[2] = event.values[2];

                handler.sendMessage(handler.obtainMessage(MSG_ID_UPDATE_gyro,gyroArray));

                /*textView_02.post(new Runnable() {
                    @Override
                    public void run() {
                        textView_02.setText("Gyr：" + gyro_x + ",\n"+gyro_y+",\n"+gyro_z+"\nAcc：\n" + accel_x + ",\n"+accel_x+",\n"+accel_z);
                    }
                });*/
            }

            //将当前时间赋值给timestamp
            //timestamp = event.timestamp;
        }



    }

    @Override

    public void onAccuracyChanged(Sensor sensor, int accuracy/*/ˈækjərəsi/准确性，精确性*/) {
        //TODO Auto-generated method stub
        Log.i(TAG, "onAccuracyChanged: sensor.name=" + sensor.getName() + "ccuracy:" + accuracy);
    }

    /**
     * A native method that is implemented by the 'agsensor' native library,
     * which is packaged with this application.
     */
    public native String stringFromJNI();
    public native float[] doMahonyAHRSupdateIMU(float[] accelArray,float[] gyroArray,int methodType/*0:MahonyAHRS or 1:SixAxisImuPose*/);
}